#include <MCPerpendicularConstraint.h>
#include <MCPerpendicularConstraintVisual.h>
#include <iostream>

using namespace cg;

MCPerpendicularConstraint::MCPerpendicularConstraint(MCSegmentVisual *s1, MCSegmentVisual *s2) :
    _segment1(s1),
    _segment2(s2),
    _visual(new MCPerpendicularConstraintVisual(_segment1->canvas(), this))
{
    _rootDir1 = _segment1->lineSegment().direction();
    _rootDir2 = _segment2->lineSegment().direction();
    _solver = CreateSolver();
    _segment1->canvas()->addVisualItem(_visual);
}

MCPerpendicularConstraint::~MCPerpendicularConstraint()
{
    RemoveSolver(_solver);
}

void MCPerpendicularConstraint::resolve()
{
    if (_segment1->lineSegment().direction() != _rootDir1)
    {
        MCLineSegment2D mcLineSegment2 = _segment2->lineSegment();
        MCVector2 vec1 = _segment1->lineSegment().direction();
        MCVector2 vec2 = _segment2->lineSegment().direction();
        makeOrthogonal(vec1, vec2);

        _segment2->update(mcLineSegment2.startPoint(), vec2, mcLineSegment2.length());
        _rootDir1 = _segment1->lineSegment().direction();
        _rootDir2 = _segment2->lineSegment().direction();

    }
    else if (_segment2->lineSegment().direction() != _rootDir2)
    {
        MCLineSegment2D mcLineSegment1 = _segment1->lineSegment();
        MCVector2 vec1 = _segment2->lineSegment().direction();
        MCVector2 vec2 = _segment1->lineSegment().direction();
        makeOrthogonal(vec1, vec2);

        _segment1->update(mcLineSegment1.startPoint(), vec2, mcLineSegment1.length());
        _rootDir1 = _segment1->lineSegment().direction();
        _rootDir2 = _segment2->lineSegment().direction();
    }
}

void MCPerpendicularConstraint::makeOrthogonal(MCVector2 &vec1, MCVector2 &vec2)
{
    Var x1 = CreateVariable(_solver, vec1.x); Var y1 = CreateVariable(_solver, vec1.y);
    Var x2 = CreateVariable(_solver, vec2.x); Var y2 = CreateVariable(_solver, vec2.y);

    Expr expression = x1 * x2 + y1 * y2;
    Eq equation = AddEquation(_solver, expression);

    if (Solve(_solver))
    {
        vec1.x = VariableValue(_solver, x1); vec1.y = VariableValue(_solver, y1);
        vec2.x = VariableValue(_solver, x2); vec2.y = VariableValue(_solver, y2);
    }

    vec1 = vec1.normalize();
    vec2 = vec2.normalize();

    RemoveEquation(_solver, equation);
    RemoveVariable(_solver, x1); RemoveVariable(_solver, x2); RemoveVariable(_solver, y1); RemoveVariable(_solver, y2);
}
